/* kraut.c */

/*-----------------------------------------------------------------------------
    Hardware configuration:
 -----------------------------------------------------------------------------*/
/*
 * Port 1: Unused
 * Port 2: Unused
 * Port 3: Color sensor
 * Port 4: Unused
 *
 * Port A: Steering motor
 * Port B: Driving motor back left
 * Port C: Driving motor back right
 */

#include <stdio.h>
#include <stdlib.h>
#include <limits.h>
#include <string.h>
#include <math.h>

#include "kernel.h"
#include "kernel_id.h"
#include "ecrobot_interface.h"

#include "KrautMobil.h"

#define PASS_KEY    "1234"  /* Bluetooth pass key */
#define RUNTIME_CONNECTION 1
#define NUMLOOPS_STEERING_END (250/50) /* 250ms total / 50 ms task activation cycle */
#define CMS_DISTANCE 23
#define CMS_DEB_LOOP 3
#define CMS_BRAKE_LOOP 100
#define CMS_PASSIVE 0
#define CMS_ACTIVE 1
#define CMS_EXECUTED 2


char cms_state;
S32 trq_cntr_req;

static void set_steering_angle(S8 angle);
static void set_torque(S8 torque);

/**
 * Hook called before the main program starts. Waits for a BT connection.
 */
void ecrobot_device_initialize()
{
#ifndef RUNTIME_CONNECTION
   ecrobot_init_bt_slave(PASS_KEY);
#endif
   ecrobot_init_sonar_sensor(NXT_PORT_S2);
   cms_state = CMS_PASSIVE;
}

/**
 * Hook called after the program shut down.
 */
void ecrobot_device_terminate()
{
   set_torque(0);
   set_steering_angle(0);
   ecrobot_term_bt_connection();
   ecrobot_term_sonar_sensor(NXT_PORT_S2);
}

/* OSEK declarations */
DeclareCounter(SysTimerCnt);

/**
 * Command dispatching function. Analyzes @a data and calls the corresponding
 * handler function.
 *
 * @param data The command data
 * @param len Length of the data buffer
 */
static void process_cmd(S8 *data, int len);



/**
 * Initialize steering so that we know min and max values
 */
static void calibrate_steering();

/**
 *
 */
typedef enum
{
   WaitingForStartByte,
   InsideMessage
} ReaderState;



/* nxtOSEK hook to be invoked from an ISR in category 2 */
void user_1ms_isr_type2(void)
{
   StatusType ercd;

   ercd = SignalCounter(SysTimerCnt); /* Increment OSEK Alarm Counter */
   if(ercd != E_OK)
   {
     ShutdownOS(ercd);
   }
}

S8 readBuffer[KRAUT_MSG_DATA_LEN] = {0,0};
U32 numDataBytesRead = 0;
ReaderState currentReaderState = WaitingForStartByte;

static struct {
   int isCalibrated;
   int isCalibrationRequested;
   int currentRequest;
   int currentPosition;
   int minPos;
   int maxPos;
} sSteeringState;

/**
 * The "main" task
 */
void read_kraut_bt_message()
{
	if (ecrobot_get_bt_status() == BT_STREAM)
	{
      S8 byteRead = 0;
	   U32 rx_len = ecrobot_read_bt(&byteRead, 0, 1);
		if (rx_len > 0)
		{
         switch(currentReaderState)
         {
         case WaitingForStartByte:
            if ( KRAUT_MSG_START_BYTE == byteRead )
            {
               memset(readBuffer, 0, KRAUT_MSG_DATA_LEN);
               numDataBytesRead = 0;
               currentReaderState = InsideMessage;
            }
            break;
         case InsideMessage:
            if ( KRAUT_MSG_END_BYTE == byteRead )
            {
               // Execute Command
               process_cmd(readBuffer, numDataBytesRead);
               currentReaderState = WaitingForStartByte;
            }
            else if ( KRAUT_MSG_START_BYTE == byteRead )
            {
               memset(readBuffer, 0, KRAUT_MSG_DATA_LEN);
               numDataBytesRead = 0;
            }
            else
            {
               readBuffer[numDataBytesRead] = byteRead;
               numDataBytesRead++;
               if ( numDataBytesRead > KRAUT_MSG_DATA_LEN )
               {
                  currentReaderState = WaitingForStartByte;
               }
            }
            break;
         }
		}
	}
}

/*----------------------------------------------------------------------------*/
static void set_torque(S8 torque)
/*----------------------------------------------------------------------------*/
{
   int brake = 1;
   if (0==torque)
   {
      // floating stop
      brake = 0;
   }

   trq_cntr_req = torque;

//   nxt_motor_set_speed(NXT_PORT_B, -torque, brake);
//   nxt_motor_set_speed(NXT_PORT_C, -torque, brake);
}

/*----------------------------------------------------------------------------*/
static void set_steering_angle(S8 angle)
/*----------------------------------------------------------------------------*/
{
   // limit range to [-100...100]
   if ( angle >  100 ) angle =  100;
   if ( angle < -100 ) angle = -100;

   // translate -100..100 request angle to actual steering range:
   int targetPosition = angle;
   targetPosition += 100;
   targetPosition = (int)((((float)sSteeringState.maxPos) / 200) * targetPosition);

   sSteeringState.currentRequest = targetPosition;
}

/*----------------------------------------------------------------------------*/
static void calibrate_steering()
/*----------------------------------------------------------------------------*/
{
   sSteeringState.isCalibrated = 0;
   nxt_motor_set_count(NXT_PORT_A, 0);
   ecrobot_set_motor_speed(NXT_PORT_A, -10);
   int countPrev = nxt_motor_get_count(NXT_PORT_A);
   sSteeringState.currentPosition = countPrev;
   do
   {
      display_goto_xy(0,3);
      display_int(sSteeringState.currentPosition,4);
      display_update();
      systick_wait_ms(200);
      countPrev = sSteeringState.currentPosition;
      sSteeringState.currentPosition = nxt_motor_get_count(NXT_PORT_A);
   }
   while(countPrev != sSteeringState.currentPosition);

   ecrobot_set_motor_speed(NXT_PORT_A, 0);

   // leftmost position is 0:
   sSteeringState.minPos = 0;
   sSteeringState.currentPosition = 0;
   nxt_motor_set_count(NXT_PORT_A, 0);

   ecrobot_set_motor_speed(NXT_PORT_A, 10);

   countPrev = nxt_motor_get_count(NXT_PORT_A);
   sSteeringState.currentPosition = countPrev;
   do
   {
      display_goto_xy(0,1);
      display_int(sSteeringState.minPos,4);
      display_goto_xy(0,2);
      display_int(sSteeringState.maxPos,4);
      display_goto_xy(0,3);
      display_int(sSteeringState.currentPosition,4);
      display_update();
      systick_wait_ms(200);
      countPrev = sSteeringState.currentPosition;
      sSteeringState.currentPosition = nxt_motor_get_count(NXT_PORT_A);
   }
   while(countPrev != sSteeringState.currentPosition);
   ecrobot_set_motor_speed(NXT_PORT_A, 0);
   sSteeringState.maxPos = sSteeringState.currentPosition;

   display_goto_xy(0,1);
   display_int(sSteeringState.minPos,4);
   display_goto_xy(0,2);
   display_int(sSteeringState.maxPos,4);
   display_goto_xy(0,3);
   display_int(sSteeringState.currentPosition,4);
   display_update();

   sSteeringState.isCalibrated = 1;
   set_steering_angle(0);
}

/*----------------------------------------------------------------------------*/
static void process_cmd(S8 *data, int len)
/*----------------------------------------------------------------------------*/
{
   if(len != KRAUT_MSG_DATA_LEN) return; //nothing to do here

   char cmd = data[0];

   display_clear(0);
   display_goto_xy(0,0);
   display_int(cmd,4);

   display_goto_xy(0,1);
   display_int(data[1],4);

   display_goto_xy(0,2);
   display_int(len,4);

   display_update();

   switch (cmd) {
      case KRAUT_CMD_REQ_TRQ:
         set_torque((S8)data[1]);
         break;

      case KRAUT_CMD_REQ_STEER:
         set_steering_angle((S8)data[1]);
         break;

      case KRAUT_CMD_REQ_STEER_CALIBRATE:
         calibrate_steering(); //sSteeringState.isCalibrationRequested = 1;
         break;

      default:
         //echo_lcd(data,len);
         //ecrobot_send_bt(data, 0, len);
         break;
   }
}

/* OSEK_Task_Background */
TASK(OSEK_Task_Background)
{
  static SINT bt_status = BT_NO_INIT;

  sSteeringState.currentPosition = 0;
  sSteeringState.currentRequest = 0;
  sSteeringState.isCalibrated = 0;
  sSteeringState.isCalibrationRequested = 0;
  sSteeringState.maxPos = 0;
  sSteeringState.minPos = 0;

  S8 cal[] = {KRAUT_CMD_REQ_STEER_CALIBRATE, 0};
  process_cmd(cal, 2);

  display_goto_xy(0,7);
  display_string("Waiting for BT");
  display_update();

  while(42)
  {
#ifdef RUNTIME_CONNECTION
    ecrobot_init_bt_slave(PASS_KEY);
#endif

    if (ecrobot_get_bt_status() == BT_STREAM && bt_status != BT_STREAM)
    {
      display_clear(0);
      display_goto_xy(0, 7);
      display_string("[BT]      ");
      display_update();

      do
      {
         read_kraut_bt_message();
      }
      while(ecrobot_get_bt_status() == BT_STREAM);
    }
    else if (ecrobot_get_bt_status() != bt_status)
    {
       display_goto_xy(0,7);
       display_string("Waiting for BT");
       display_update();
    }
    bt_status = ecrobot_get_bt_status();
  }
}

#define EQUALS(a,b) (abs((a)-(b)) <= 1)

void check_distance_sensor();

TASK(SteeringControl)
{
   static int stickCounter = 0;

   display_goto_xy(0,4);
   display_string("req");
   display_goto_xy(4,4);
   display_int(sSteeringState.currentRequest,4);

   display_goto_xy(0,5);
   display_string("cur");
   display_goto_xy(4,5);
   display_int(sSteeringState.currentPosition, 4);
   display_update();

//   calibrate_steering() calls sleep() which does not seem to work well with
//   cyclic tasks.
//   if ( 1 == sSteeringState.isCalibrationRequested )
//   {
//      calibrate_steering();
//      sSteeringState.isCalibrationRequested = 0;
//   }

   if ( 1 == sSteeringState.isCalibrated )
   {
      if ( !EQUALS(sSteeringState.currentRequest, sSteeringState.currentPosition) )
      {
         int posDelta = abs(sSteeringState.currentPosition - sSteeringState.currentRequest);
         int speed = (posDelta * 1/4)+10;
         if (sSteeringState.currentPosition < sSteeringState.currentRequest)
         {
            ecrobot_set_motor_speed(NXT_PORT_A, speed);
         }
         else
         {
            ecrobot_set_motor_speed(NXT_PORT_A,-speed);
         }

         int newPos = sSteeringState.currentPosition;

         display_goto_xy(0,3);
         display_int(newPos,4);

         display_update();

         newPos =  nxt_motor_get_count(NXT_PORT_A);
         if (sSteeringState.currentPosition == newPos)
         {
            stickCounter++;
            if (stickCounter > NUMLOOPS_STEERING_END )
            {
               ecrobot_set_motor_speed(NXT_PORT_A, 0);
               stickCounter = 0;
            }
         }
         else
         {
            stickCounter = 0;
         }
         sSteeringState.currentPosition = newPos;
      }
      else
      {
         ecrobot_set_motor_speed(NXT_PORT_A, 0);
         stickCounter = 0;
      }
   }

   // Check ultrasonic sensor value and stop if an obstacle has been detected

   switch(cms_state)
   {
   case CMS_EXECUTED:
      trq_cntr_req = (trq_cntr_req > 0)? 0 : trq_cntr_req;
      break;
   case CMS_ACTIVE:
      trq_cntr_req = 0;
   break;
   }

   display_goto_xy(0,6);
   display_int(cms_state,4);

   display_goto_xy(5,6);
   display_int(ecrobot_get_sonar_sensor(NXT_PORT_S2),4);
   nxt_motor_set_speed(NXT_PORT_B, -trq_cntr_req, (cms_state == CMS_ACTIVE));
   nxt_motor_set_speed(NXT_PORT_C, -trq_cntr_req, (cms_state == CMS_ACTIVE));

   TerminateTask();
}

void check_distance_sensor(void)
{

}

TASK(SensorRefreshAndUpdate)
{
   S32 usValue = ecrobot_get_sonar_sensor(NXT_PORT_S2);
   static S32 counter = 0;

   switch(cms_state)
   {
   case CMS_PASSIVE:
      if(usValue < CMS_DISTANCE) counter++;
      else counter = 0;

      if(counter > CMS_DEB_LOOP) cms_state = CMS_ACTIVE;
      break;
   case CMS_ACTIVE:
      counter++;
      if(counter > CMS_DEB_LOOP + CMS_BRAKE_LOOP) cms_state = CMS_EXECUTED;
      break;
   case CMS_EXECUTED:
      if(usValue >= CMS_DISTANCE+5)
      {
         cms_state = CMS_PASSIVE;
         counter = 0;
      }
      break;
   default:
      cms_state = CMS_PASSIVE;
      counter = 0;
      break;
   }

   TerminateTask();
}
